#include "heapsort.hpp"
#include <algorithm>

#define parent(i) ((i-1)/2)
#define left(i)   ((i<<1)+1)
#define right(i)  ((i+1)<<1)

static void dump_heap(vector<int> &a)
{
    vector<int>::iterator it;
    cout << "heap dump" << endl;
    for ( it=a.begin() ; it < a.end(); it++ )
            cout << " " << *it;
    cout << endl << endl;
}

// heapify the range a[0] - a[max]
static void heapify(vector<int> &a, int min, int max)
{
    if (debug) {
        cout << "heapify element " << min << " to " << max << endl;
        dump_heap(a);
    }

    if (max < 1 || max > (int)a.size()-1)
        return;

    int l = a[left(min)];
    int r = a[right(min)];
    int idx = min;
    int larger = a[min];
    if (left(min) <= max && l > larger) {
        idx = left(min);
        larger = l;
    }
    if (right(min) <= max && r > larger) {
        idx = right(min);
        larger = r;
    }
    if (idx != min) {
        // exchange a[idx] and a[min]
        int tmp = a[idx];
        a[idx] = a[min];
        a[min] = tmp;
        heapify(a, idx, max);
    }
    return;
}

static void build_heap(vector<int> &a)
{
    for (int i = a.size()/2 - 1; i >= 0; i--)
        heapify(a, i, (int)a.size()-1);
    if (debug) {
        cout << "heap built complete" << endl;
        dump_heap(a);
    }
}


void heapsort(vector<int> &a)
{
    if (a.size() <= 1) return;

    build_heap(a);

    for (int i = a.size()-1; i>0; i--)
    {
        int tmp = a[i];
        a[i] = a[0];
        a[0] = tmp;
        heapify(a, 0, i-1);
    }
}

//Heap sort using STL heap functions
/*
void heapsort(vector<int> &a) {
    if (a.size() <=1) return;

    make_heap(a.begin(), a.end());
    for (size_t i = 0; i < a.size() - 1; i++) {
        pop_heap(a.begin(), a.end()-i);
    }
}*/
